"""
@breif 走直线
@author Stone at BlueNet, GDOU
@date Nov. 2020
"""
import numpy as np
import logging

import settings as st
from camera import IMG_WIDTH, IMG_HEIGHT
from control.task import Task, TASK_LOOP
from vision import lane_detect
# from pid import PIDController


logger = logging.getLogger(__name__)


# ===== ===== ===== ===== =====
# contants initialization
# ===== ===== ===== ===== =====
IMG_WIDTH, IMG_HEIGHT = IMG_WIDTH, IMG_HEIGHT
SEG_Y = int(IMG_HEIGHT/3)  # 图像上下区域的分割线



class TaskGoStraight(Task):
    def __init__(self, video_capture):
        Task.__init__(self, task_type=TASK_LOOP)
        
        self.cap = video_capture
        # self.pid = PIDController() 
        self.control_center = int(IMG_WIDTH/2), int(IMG_HEIGHT/2)
        self.x_limit_lower = 90  # pixels, 允许 control_center 距离车道线的最小值
        self.x_limit_upper = 140 # pixels, 允许 control_center 距离车道线的最大值
        self.out_err        = st.V_MV_ERR
        self.out_turn_left  = st.V_MV_TURN_LEFT
        self.out_turn_right = st.V_MV_TURN_RIGHT
        self.out_no_change  = st.V_MV_NO_CHANGE
    
    def work2(self):
        ret, raw_img = self.cap.read()
        if ret:        
            bw = lane_detect.pre_canny(raw_img, eq_hist=True)
                
            seg_y = 200
            cx, cy = 320, 240
            lane_left, lane_right = lane_detect.lane_by_hough(bw, seg_y=seg_y)
            lleft, lright = lane_detect.lane_coordinate(copy(lane_left), copy(lane_right), center=(cx, cy))
        
            lane_detect.draw(raw_img, bw, lane_left, lane_right, lleft, lright, (cx, cy))

    def work(self):
        ret, raw_img = self.cap.read()
        
        if ret:
            bw_edge = lane_detect.pre_canny(raw_img, eq_hist=True)
            # detect lane
            # TODO make a adaptable seg_y
            lane_left_kb, lane_right_kb = lane_detect.lane_by_hough(bw_edge, seg_y=SEG_Y, th_votes=75, 
                min_theta=np.pi*5/180, max_theta=np.pi*60/180)
            lane_left_kx, lane_right_kx = lane_detect.lane_coordinate(lane_left_kb, lane_right_kb, self.control_center)
            
            out = self.control_policy(lane_left_kx, lane_right_kx)
            if __debug__:
                lane_detect.draw(raw_img, bw_edge, lane_left_kb, lane_right_kb, lane_left_kx, lane_right_kx, self.control_center)
        else:
            logging.info('read camera failed')
            
            out = self.out_err  # 因为摄像头出错, 所以输出0 (停机)
        
        # print('lane_left:{}, lane_right: {}, out:{} '.format(lane_left_kx, lane_right_kx, out))  # debug
        # print('out:{} '.format(out))  # debug
        return st.LB_MOVE, [out]
    
    def control_policy(self, lane_left, lane_right):
        """当控制中心位置在限制范围内时, 不改变电机运动, 当控制中心位置超出限制
        范围时, 控制电机向相反方向动作."""
        if lane_left is not None :
            k1, x1 = lane_left
            if x1 < -self.x_limit_upper:
                out = self.out_turn_left
            elif x1 > -self.x_limit_lower:
                out = self.out_turn_right
            else:
                out = self.out_no_change
        
        elif lane_right is not None:
            k2, x2 = lane_right
            if x2 < self.x_limit_lower:
                out = self.out_turn_left
            elif x2 > self.x_limit_upper:
                out = self.out_turn_right
            else:
                out = self.out_no_change
        
        else:
            out = self.out_err
        
        return out



if __name__ == '__main__':
    dir_ = os.path.dirname(__file__)
    dir_ = os.path.join(dir_, r'../../img_and_videos')
    
    # pth_vid = os.path.join(dir_, r'tf-light-noon.mp4')
    pth_vid = os.path.join(dir_, r'11-28-0522.avi')
    cap = cv2.VideoCapture(pth_vid)
    # cap = cv2.VideoCapture(1)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
    
    while True:
        ret, raw = cap.read()
        cv2.imshow('raw', raw)
        if not ret:
            print('camera fail')
            exit(0)
        
        # img_size: 320x240
        lights = detect_traffic_light(raw, seg_y=240/4, r_light_min=8, r_light_max=20)
        print(lights)
        
        # cv2.imshow('raw', raw)
        if cv2.waitKey(20) & 0xFF == 27:  # Esc
            exit()

